/*
 * @FilePath: fast_planner.hpp
 * @Author: wyx
 * @Date: 2023-09-22 20:50:49
 * @LastEditors:
 * @LastEditTime: 2024-3-20 
 * Copyright: 2023 xxxTech CO.,LTD. All Rights Reserved.
 * @Descripttion: 
 */
#pragma once

#ifndef __FAST_PLANNER_HPP_
#define __FAST_PLANNER_HPP_

#include<rclcpp/rclcpp.hpp>
#include<geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include<geometry_msgs/msg/pose_stamped.hpp>
#include<nav_msgs/msg/occupancy_grid.hpp>
#include<nav_msgs/msg/path.hpp>
#include<eigen3/Eigen/Eigen>
#include<visualization_msgs/msg/marker.hpp>
#include<visualization_msgs/msg/marker_array.hpp>
#include<iostream>
#include<stdio.h>
#include<fstream>
#include<string>
#include<time.h>
#include<sys/time.h>
#include<memory>
#include"poly_traj_optimizer.h"
#include "base_interface/usr_text_output.hpp"

// #include "cubic_spline.hpp"
// #include "topp.hpp"
// #include "topp.h"

using namespace std;
using namespace std::placeholders;
using namespace Eigen;

namespace fast_planner 
{


    class FastPlan
    {
    public:
        FastPlan() 
        {    
            // timer_ = this->create_wall_timer(20ms,std::bind(&FastPlan::timer_callback_test,this));
            std::make_shared<FileOstream>(minco_traj, "data", "minco_txt", "minco_traj");
            std::make_shared<FileOstream>(plan_state, "data", "minco_txt", "plan_state");
            // std::make_shared<FastPlan>
            // FastPlan::Ptr test_function;
        }
    // private:
    public:

        MatrixXd MincoPlanManage( float cv_time, 
                              const Vector2d& Current_Pos,//现在位置 
                              const Vector2d& Current_Vel,//现在速度
                              const Vector2d& Current_Acc,//现在加速度
                              const Vector2d& Target_Pos, //目标位置
                              const Vector2d& Target_Vel, //目标速度
                              const Vector2d& Target_Acc  //目标加速度
                                                                    );
        // void getWayPoints(Eigen::Matrix2Xd& innerPoints, nav_msgs::msg::Path& path); 
        void TrapezoidAllocateTime(const MatrixXd &wayPs,VectorXd& time_vec, double vel, double acc);
        void getMincoTraj(poly_traj::MinJerkOpt& best_MJO, const Eigen::VectorXd& time_vec, const Eigen::Matrix2Xd& innerPoints); 
        rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr global_traj_pub_;
     
        //输出文件名
        ofstream minco_traj;
        ofstream plan_state;


        Vector2d begin_pose_ = Vector2d::Zero();//x,y初始位置
        Vector2d begin_vel_  = Vector2d::Zero();//x,y初始速度
        Vector2d begin_acc_  = Vector2d::Zero();//x,y初始加速度

        Vector2d end_pose_   = Vector2d::Zero();//x,y终止位置
        Vector2d end_vel_    = Vector2d::Zero();//x,y终止速度
        Vector2d end_acc_    = Vector2d::Zero();//x,y终止加速度


        int seg_num_;//轨迹段数
        double seg_len_ = 0.05;//每段初始轨迹的长度

        // /*topp related*/
        // Eigen::Vector3d start_pose_;
        // double max_vel_ = 15.0;
        // double topp_max_acc_ = 6.0;//topp最大加速度
        // int topp_reso_ = 5; //topp精度，值越大精度越高

        /*Minco optimization*/
        ego_planner::PolyTrajOptimizer::Ptr global_ploy_traj_opt_;
        int global_cps_num_prePiece_ = 10; //每段轨迹控制点的数量
        double wei_feas_ = 1000000; //动力学权重，包含最大速度、加速度、jerk
        double max_vel_ = 5.0;
        double minco_max_acc_ = 3.5;//minco最大加速度
        // double minco_max_acc_ = 2.0;//minco最大加速度
        double seg_dt_ = 0.05;



    };
}





#endif


